#ifndef RM_AIM__BALLISTIC_SOLVER_CORE_HPP_
#define RM_AIM__BALLISTIC_SOLVER_CORE_HPP_

// 必要依赖头文件
#include "geometry_msgs/msg/point.hpp"
#include "aim_interfaces/msg/target.hpp"
#include <cmath>

namespace rm_aim
{
    // -------------------------- 核心弹道解算类（无内置参数，纯算法封装） --------------------------
    class BallisticSolverCore
    {
    public:
        // 无参构造
        BallisticSolverCore() = default;

        // -------------------------- 纯算法静态方法声明 --------------------------
        /**
         * @brief 计算目标水平距离（x-y平面）
         * @param target 目标3D坐标
         * @return 水平距离（m）
         */
        double calcHorizontalDist(const geometry_msgs::msg::Point &target);

        /**
         * @brief 计算目标3D距离
         * @param target 目标3D坐标
         * @return 3D距离（m）
         */
        double calc3dDist(const geometry_msgs::msg::Point &target);

        /**
         * @brief 解算Yaw角（rad）- 纯几何计算（atan2(y, x)）
         * @param target 目标3D坐标
         * @return Yaw角（rad，范围：[-π, π]）
         */
        double solveYaw(const geometry_msgs::msg::Point &target);

        /**
         * @brief 解算Pitch角（rad）- 含重力+空气阻力补偿
         * @param target 目标3D坐标（x:前, y:左, z:上）
         * @param bullet_speed 子弹初速（m/s）
         * @param gravity 重力加速度（m/s²，默认9.8）
         * @param k1 空气阻力系数（默认0.0）
         * @return 解算后的Pitch角（rad，向上为正）
         */
        double solvePitch(
            const geometry_msgs::msg::Point &target,
            double bullet_speed,
            double gravity = 9.8,
            double k1 = 0.0);

        /**
         * @brief 计算飞行时间（s）- 空气阻力指数模型
         * @param horizontal_dist 目标水平距离（m）
         * @param bullet_speed 子弹初速（m/s）
         * @param pitch_rad Pitch角（rad）
         * @param k1 空气阻力系数
         * @return 飞行时间（s）
         */
        double calcFlightTime(
            double horizontal_dist,
            double bullet_speed,
            double pitch_rad,
            double k1);

        /**
         * @brief 预测未来装甲板位置（基于目标速度和角速度）
         * @param target 跟踪目标数据（含位置、速度、偏航角、装甲板半径）
         * @param dt 预测时间间隔（s）
         * @return 预测后的装甲板3D坐标
         */
        geometry_msgs::msg::Point predictArmorPos(
            const aim_interfaces::msg::Target &target,
            double dt);

        /**
         * @brief 迭代求解精确瞄准点（补偿总延迟+飞行时间）
         * @param target 跟踪目标数据
         * @param total_delay 总延迟
         * @param flight_time_weight 飞行时间权重
         * @param bullet_speed 子弹初速（m/s）
         * @param gravity 重力加速度（m/s²，默认9.8）
         * @param k1 空气阻力系数（默认0.0）
         * @return 精确瞄准点3D坐标
         */
        geometry_msgs::msg::Point calcPreciseAimPoint(
            const aim_interfaces::msg::Target &target,
            double total_delay,
            double flight_time_weight,
            double bullet_speed,
            double gravity = 9.8,
            double k1 = 0.0);
    };
} // namespace rm_aim

#endif // RM_AIM__BALLISTIC_SOLVER_CORE_HPP_